#!/home/robot/anaconda3/envs/yolograsp/bin/python3
import sys, os
sys.path.insert(1, os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../..')) # src/
from utilities.tools.image_converter import convert_msg_to_nparray
sys.path.pop(1)
import open3d as o3d
import numpy as np
import rospy
import utilities.ros_np_multiarray as ros_nm
import math
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import Image
from shape_msgs.msg import Plane


class Plane_Detect_Server:
    def __init__(self):
        self.cam_fx, self.cam_fy, self.cam_cx, self.cam_cy = 603.1332397460938, 601.6819458007812, 323.1732177734375, 241.5373077392578
        self.norm_scale = 1000.0
        img_width = 640
        img_height = 480
        self.xmap = np.array([[i for i in range(img_width)] for j in range(img_height)])
        self.ymap = np.array([[j for i in range(img_width)] for j in range(img_height)])
        rospy.Service("/perception/plane_detect", None, self.callback)
        rospy.wait_for_service("/perception/plane_detect")

    def callback(self, req):
        # TODO: write it when it is needed
        pass


    
if __name__ == '__main__':
    rospy.init_node('plane_detection', anonymous=True)
    plane_detection = Plane_Detect_Server()
    rospy.spin()
